/*
实现功能：
1.一键起飞/停止 ： 地面端launch一个文件即可 node里面让数传发送数据给飞机端
2.显示数据 定一个Fly_Num 进行布局  显示Num个飞机的 3 * float 64 位数据

计划安排：
1.学习 QT布局
2.补充学习文本的更新
3.实现话题的订阅
4.msg文件定义及传输

*/
#include "rviz_plugin.h"

#include <stdio.h>
#include <string>
#include <sstream>
#include <time.h>
#include "std_msgs/String.h"
//QT类控件声明
#include <QLabel>
#include <QPainter>
#include <QLineEdit>
#include <QPushButton>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QTextBrowser>
#include <QTabWidget>
#include <QSpacerItem>
#include <QTimer>
#include <QProcess>
#include <QDebug>
#include <QFrame>

#include <QTableWidget>
#include <QTableWidgetItem>

#include <QHeaderView>

#define pilot_nume 4;

QProcess *take_off_Process = new QProcess;
QProcess *off_Process = new QProcess;



/*构造函数*/
QTableWidget *tableWidget = new QTableWidget(3,6);
namespace rviz_plugin
{
 Invincible::Invincible(QWidget* parent)
 :rviz::Panel(parent)
 {

    /*重置窗口大小*/
    resize(800,600);
    setFixedSize(800,600);
    setWindowTitle("无人争锋rviz控制台");


    /*一键起飞/停止*/
     push_button_takeoff = new QPushButton("Take_off");
     push_button_takeoff->setParent(this);
     push_button_takeoff->setText("一键起飞");
     push_button_takeoff->setStyleSheet("background-color:rgb(255,255,255)");
    connect(push_button_takeoff,SIGNAL(clicked()),this,SLOT(robot_takeoff() ));

     push_button_off = new QPushButton("Take_off");
     push_button_off->setParent(this);
     push_button_off->setText("一键停止");
     push_button_off->setStyleSheet("background-color:rgb(255,255,255)");
     connect(push_button_off,SIGNAL(clicked()),this,SLOT(robot_off() ));


   /*按钮布局层*/
     QHBoxLayout *btn_Layout = new QHBoxLayout();
      btn_Layout->addWidget(push_button_takeoff);
      btn_Layout->addStretch(); 
      btn_Layout->addWidget(push_button_off);
      btn_Layout->setContentsMargins(5,5,5,5);
      /*状态布局层*/
   QHBoxLayout *state_Layout = new QHBoxLayout();

    QLabel *s1=new QLabel(this) ;
    QLabel *s2=new QLabel(this) ;
    QLabel *s3=new QLabel(this) ;
    QLabel *s4=new QLabel(this) ;
    s1->setText("正常飞行");
    s2->setText("通讯情况");
    s3->setText(" state3");
    s4->setText("state4 ");
    state_Layout->addWidget(s1);
    state_Layout->addStretch(); 
    state_Layout->addWidget(s2);
    state_Layout->addStretch();  
    state_Layout->addWidget(s3);
    state_Layout->addStretch(); 
    state_Layout->addWidget(s4);    
    state_Layout->setContentsMargins(5,5,5,5);

     //小布局
     QVBoxLayout *small_Layout = new QVBoxLayout();
     QLabel *A=new QLabel(this) ;
     QLabel *L=new QLabel(this) ;
     QLabel *H=new QLabel(this) ;
     QLabel *NoneN=new QLabel(this) ;
     NoneN->setText("   ");
     A->setText("经度");
     L->setText("纬度");
     H->setText("海拔");
     small_Layout->addWidget(NoneN);
     small_Layout->addWidget(A);
     small_Layout->addWidget(L);
     small_Layout->addWidget(H);
     small_Layout->addStretch();

    /*数据表格布局层*/
     QHBoxLayout *table_Layout = new QHBoxLayout();
      //  QTableWidget *tableWidget = new QTableWidget(5,8);
      // tableWidget->setWindowTitle("无人机经纬海拔数据");
      //  QStringList header;
      // header<<"无人机经纬海拔数据";
      // tableWidget->setHorizontalHeaderLabels(header);
      tableWidget->setItem(0,0,new QTableWidgetItem("00.000000"));
      tableWidget->setItem(0,1,new QTableWidgetItem("00.000000"));
      tableWidget->setItem(0,2,new QTableWidgetItem("00.000000"));
      tableWidget->setItem(0,3,new QTableWidgetItem("00.000000"));
      tableWidget->setItem(0,4,new QTableWidgetItem("00.000000"));
      tableWidget->setItem(0,5,new QTableWidgetItem("00.000000"));
      // tableWidget->setItem(0,6,new QTableWidgetItem("00.000000"));
      // tableWidget->setItem(0,7,new QTableWidgetItem("00.000000"));
      //tableWidget->setItem(1,1,new QTableWidgetItem("px1"));
      //  tableWidget->item(1,1)->setText(res);
    //   tableWidget->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); 
    //   tableWidget->horizontalHeader()->setSectionResizeMode(0, QHeaderView::ResizeToContents);   
      tableWidget->show();
      //  table_Layout->addStretch(); 
      table_Layout->addItem(small_Layout);
      table_Layout->addWidget(tableWidget);
      table_Layout->setContentsMargins(10,10,2,2);
      // table_Layout->addStretch(); 
      //总体布局

      QVBoxLayout *All_Layout=new QVBoxLayout;
      All_Layout->addItem(btn_Layout);
      All_Layout->addItem(state_Layout);
      All_Layout->addItem(table_Layout);
      All_Layout->addStretch();
      this->setLayout(All_Layout);

    //  订阅消息，进入回调函数
      subscriber_geomsgs = nh_.subscribe("/all_uavs_position/global", 10, &Invincible::msgs_callback, this);

 } 


//包含构造函数一共6个函数
/*获得订阅的数据*/

/* 展示订阅的数据*/

/*利用QProcess一键起飞*/
 void  Invincible::robot_takeoff(void)
 {
    QString program = "roslaunch turtlesim_pkg rviz_test.launch ";
    take_off_Process->start(program);
    push_button_takeoff->setStyleSheet("background-color:rgb(0,255,0)");
    push_button_off->setStyleSheet("background-color:rgb(255,255,255)");
 }

/*利用QProcess一键停止*/
  void  Invincible::robot_off(void)
 {
    push_button_off->setStyleSheet("background-color:rgb(255,0,0)");
    push_button_takeoff->setStyleSheet("background-color:rgb(255,255,255)");
    QString program = "killall -9 roscore;killall -9 rosmaster ";
    off_Process->start(program);
 }




/*Topic的输入及更新*/
/*
void Invincible::updateTopic()
{
  setTopic( output_topic_editor_->text() );
}
 void Invincible::set_topic(const QString& topic)
 {
  	setTopic( output_topic_editor_->text() );
 }
 */

/*待修改*///如果都发布到一个话题上 如何通过不同的发布者判断输入到几行 还是说发布到不同的话题上
    void Invincible::msgs_callback(const serialport::ALL_UAVs_LLA& data)
    {
     int length= sizeof(data.all_uavs_lla)/sizeof(data.all_uavs_lla[0]);
     length=4;
      float test=float(length);
     num_to_char (test,2,5);//行列
     length--;
    for(;length>=0;length--)
    {
      float la;
      float l;
      float h;
      la= data.all_uavs_lla[length].latitude;
      l= data.all_uavs_lla[length].longitude;
      h= data.all_uavs_lla[length].altitude;
        num_to_char (l,0,length);//行列
        num_to_char (la,1,length);
        num_to_char (h,2,length);
    }
      float tr;
      std_msgs::String res;
      std::stringstream ss;
      tr= data.all_uavs_lla[0].altitude;
      ss<<tr;;
      res.data= ss.str();
      tableWidget->setItem(1,1,new QTableWidgetItem(res.data.c_str()));
    }

   void  Invincible::num_to_char (float data,int length,int i)
   {
      char to_char[15];
      sprintf(to_char, "%012.8f",data);  
      tableWidget->setItem(length,i,new QTableWidgetItem(to_char));
   }






/*rviz配置段*/
void Invincible::save( rviz::Config config ) const
{
  rviz::Panel::save( config );
  config.mapSetValue( "Topic", output_topic_ );
}

// 重载父类的功能，加载配置数据
void Invincible::load( const rviz::Config& config )
{
  rviz::Panel::load( config );
  QString topic;
  if( config.mapGetString( "Topic", &topic ))
  {
  }
}


 }//namespace end
    // 声明此类是一个rviz的插件
#include <pluginlib/class_list_macros.h>
 PLUGINLIB_EXPORT_CLASS(rviz_plugin::Invincible,rviz::Panel )
